#ifndef IHGAIT_CTRLLER_H
#define IHGAIT_CTRLLER_H

#include <math.h>
#include <time.h>

#include <iostream>
#include <string>

#include "ihDynamics/IronHorse.h"
#include "ihControllers/ihLegController.h"
#include "ihControllers/ihControlFSMData.h"
#include "ihControllers/ihSafetyChecker.h"
#include "ihMPC_Ctrl/ihConvexMPCLocomotion.h"

#include "Controllers/ContactEstimator.h"
//#include "Controllers/ControlFSMData.h"
#include "Controllers/DesiredStateCommand.h"
#include "Controllers/OrientationEstimator.h"
#include "Controllers/PositionVelocityEstimator.h"
#include "Controllers/RobotLegState.h"
#include "Controllers/StateEstimatorContainer.h"
//#include "Controllers/SafetyChecker.h"
//#include "Dynamics/MiniCheetah.h"
//#include "MPC_Ctrl/ConvexMPCLocomotion.h"
#include "Utilities/IMUTypes.h"
#include "calculateTool.h"

//#include <rbdl/rbdl.h>

using namespace RigidBodyDynamics;

struct ihJointEff {
  double eff[12];
};

class ihGaitCtrller {
 public:
  ihGaitCtrller(double freq, double* PIDParam);
  ~ihGaitCtrller();
  void ihSetIMUData(double* imuData);
  void ihSetLegData(double* motorData);
  void ihPreWork(double* imuData, double* motorData);
  void ihSetGaitType(int gaitType);
  void ihSetRobotMode(int mode);
  void ihSetRobotVel(double* vel);
  void ihToqueCalculator(double* imuData, double* motorData, double* effort);

 private:
  int _gaitType = 0;
  int _robotMode = 0;
  bool _safetyCheck = true;
  std::vector<double> _gamepadCommand;
  Vec4<float> ctrlParam;

  //Model* _ironhorse;

  Quadruped<float> _quadruped;
  ConvexMPCLocomotion* convexMPC;

  ihLegController<float>* _legController;
  ihSafetyChecker<float>* safetyChecker;

  StateEstimatorContainer<float>* _stateEstimator;
  LegData _legdata;
  LegCommand legcommand;
  ControlFSMData<float> control_data;
  VectorNavData _vectorNavData;
  CheaterState<double>* cheaterState;
  StateEstimate<float> _stateEstimate;
  RobotControlParameters* controlParameters;
  DesiredStateCommand<float>* _desiredStateCommand;
  //SafetyChecker<float>* safetyChecker;
};

extern "C" {

ihGaitCtrller* ihgCtrller = NULL;
ihJointEff ih_JointEff;

// first step, init the controller
void init_controller_ih(double freq, double PIDParam[]) {
  if (NULL != ihgCtrller) {
    delete ihgCtrller;
  }
  ihgCtrller = new ihGaitCtrller(freq, PIDParam);
}

// the kalman filter need to work second
void pre_work_ih(double imuData[], double legData[]) {
  ihgCtrller->ihPreWork(imuData, legData);
}

// gait type can be set in any time
void set_gait_type_ih(int gaitType) { ihgCtrller->ihSetGaitType(gaitType); }

// set robot mode, 0: High performance model, 1: Low power mode
void set_robot_mode_ih(int mode) { ihgCtrller->ihSetRobotMode(mode); }

// robot vel can be set in any time
void set_robot_vel_ih(double vel[]) { ihgCtrller->ihSetRobotVel(vel); }

// after init controller and pre work, the mpc calculator can work
ihJointEff* toque_calculator_ih(double imuData[], double motorData[]) {
  double eff[12] = {0.0};
  ihgCtrller->ihToqueCalculator(imuData, motorData, eff);
  for (int i = 0; i < 12; i++) {
    ih_JointEff.eff[i] = eff[i];
  }
  return &ih_JointEff;
}
}

#endif